#ifndef WRMNK_H
#define WRMNK_H

#include <iostream>
#include <vector>
#include <string>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>


namespace ublas = boost::numeric::ublas;

namespace Jano {

// Deklaracja zapowiadająca
class WRMNK;

/** Alias na zręczny wskaźnik dla klasy ControllerBase */
typedef boost::shared_ptr<WRMNK> WRMNKSPtr;

class WRMNK
{
private:
    double m_dA; //! Stopień identyfikowanego wielomianu A
    double m_dB; //! Stopień identyfikowanego wielomianu B
    double m_k; //! Czas opóźnienia identyfikowanego obiektu
    double m_lambda; //! Współczynnik zapominania z zakresu (0;1]
    ublas::vector<double> m_vecTheta; //! Wektor parametrow identyfikacji
    ublas::vector<double> m_vecInitTheta; //! Początkowy wektor par. id.
    ublas::vector<double> zeroVec;
    ublas::vector<double> m_memoryU;

    // Wszystkie statyczne wylądowały tutaj
    double epsilon, beta ;
    ublas::vector<double> nextTheta, previousTheta, fi, k;
    ublas::identity_matrix<double> I;
    ublas::matrix<double> previousP, nextP;
    ublas::matrix<double> zero;
    int m;

    ublas::matrix<double> numinator;
    ublas::matrix<double> fraction;

    double sizeX;
    double tempSize;

    /*! \brief Odwracanie macierzy
     *
     * Funkcja umożliwiająca odwrócenie macierzy
     *
     * \param input - macierz wejściowa do obrócenia
     * \param inverse - wynikowa macierz odwrócona
     * \return informacja o powodzeniu operacji
     */
    template<class T>
    bool InvertMatrix(const ublas::matrix<T>& input, ublas::matrix<T>& inverse);
    /*! \brief Aktulizacja wektora regresyjnego
     *
     * Prywatna funkcja odpowiedzialna za aktualizację w każdym kroku identyfikacji
     * stanu wektora regresyjnego zawieracjącego próbki sygnału wejściowego i wyjściowego
     * identyfikowanego obiektu.
     *
     * \param u - próbka sygnału wejściowego
     * \param y - próbka sygnały wyjściowego obiektu
     * \return zaaktualizowany wektor regresyjny
     */
    template<class T>
    ublas::vector<T> updateFI(T u, T y, ublas::vector<T> fi);
    /*! \brief Funkcja konwertująca
     *
     * Prywatna funkcja umożliwiająca konwersię wektora z przestrzeni ublas
     * na wektor z przestrzeni std;
     *
     * \param ublasVec - wektor z przestrzeni ublas do konwersji
     * \return wektor z przestrzeni std
     */
    template<class T>
    std::vector<T> convertVectorUblasToSTD(ublas::vector<T> ublasVec);
    /*! \breif Algorytm RMNK z kierunkowym zapominaniem
     *
     * Funkcja składowa klasy realizująca algorytm RMNK z kierunkowym zapominaniem
     * w celu uniknięcia zjawiska zwanego wybuchem estymatora.
     *
     * \param nPreviousP - poprzednia macierz kowariancji
     * \param nFi - poprzedni wektor regresyjny
     * \return aktualna macierz kowariancji
     */
    template<class T>
    ublas::matrix<T> directionalForgeting(ublas::matrix<T> &nPreviousP, ublas::vector<T> &nFi);
public:
    WRMNK(double nk, double ndA, double ndB, double nlambda);
    ~WRMNK();

    /*! \brief Identyfikacja WRMNK
     *
     * Funkcja umożliwiająca dokonanie identyfikacji obiektu zgodnie z
     * rekurencyjną ważoną metodą najmniejszych kwadratów
     *
     * \param objOutY - aktualna próbka sygnału wyjściowego identyfikowanego obiektu
     * \param regOutU - aktulana próbka sterowania pochodząca z regulatora będąca wejściem obiektu
     * \return Zidentyfikowane parametry w chwili i
     */
    std::vector<double> identification(double objOutY, double regOutU, double gamma);
    /*! \brief Odczytanie zidentyfikowanych parametrów
     *
     * Funkcja umożliwia odczyt parametrów identyfikacji
     *
     * \param factors - określa który z wielomianół chcemy odczytać
     * \return wybrany wielomian
     */
    std::vector<double> getPolynomialFactors(std::string factors);
    /*! \brief Ustawianie parametrów
     *
     * Procedura umożliwiająca ustawienie konfiguracji identyfikowanego obiektu.
     *
     * \param nk - opóźnienie obiektu
     * \param ndA - stopień wielomianu A(z^-1)
     * \param ndB - stopień wielomianu B(z^-1)
     * \param nlambda - współczynnik zapominania
     */

    std::vector<double> getAPoly();
    std::vector<double> getBPoly();

    void setInitVal(const std::vector<double>& initA, const std::vector<double>& initB);

    void setStructure(double nk, double ndA, double ndB, double nlambda);
    /*! \brief Pobranie parametru k
     *
     * \return opóźnienie identyfikowanego obiektu
     */
    double getK();
    /*! \brief Pobranie stopnia wielomianu A
     *
     * \return stopień wielomianu A identyfikowanego obiektu
     */
    double getdA();
    /*! \brief Pobranie stopnia wielomianu B
     *
     * \return stopień wielomianu B identyfikowanego obiektu
     */
    double getdB();
    /*! \brief Pobranie współczynnika zapominania algorytmu
     *
     * \return współczynnik zapominania
     */
    double getLambda();
};

} // namespace Jano

#endif // WRMNK_H
